﻿#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QThread>
#include <QVector3D>
#include <QtMath>
#include <QDebug>
#include <QFileDialog>
#include <QThread>
#include "lua_script.h"

QVector<double> string2vector(QString str)
{
    QVector<double> vec;
    QStringList list=str.trimmed().split(",");
    for(int i=0;i<list.size();++i)
    {
        QString temp=list.at(i).trimmed();
        vec.push_back(temp.toDouble());
    }
    return vec;
}

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    QVector<QPushButton*> buttons;
    buttons<<ui->pushButton_j1_left<<ui->pushButton_j2_left<<ui->pushButton_j3_left<<ui->pushButton_j4_left<<ui->pushButton_j5_left<<ui->pushButton_j6_left
            <<ui->pushButton_j1_right<<ui->pushButton_j2_right<<ui->pushButton_j3_right<<ui->pushButton_j4_right<<ui->pushButton_j5_right<<ui->pushButton_j6_right
            <<ui->pushButton_x_left<<ui->pushButton_y_left<<ui->pushButton_z_left<<ui->pushButton_a_left<<ui->pushButton_b_left<<ui->pushButton_c_left
            <<ui->pushButton_x_right<<ui->pushButton_y_right<<ui->pushButton_z_right<<ui->pushButton_a_right<<ui->pushButton_b_right<<ui->pushButton_c_right;
    for(int i=0;i<buttons.size();++i)
    {
        connect(buttons.at(i),SIGNAL(released()),Taike::PickMoveZ::Instance(),SLOT(stopJog()));
    }
    refrshtimer=new QTimer(this);
    connect(refrshtimer,SIGNAL(timeout()),this,SLOT(slot_refresh()));
    script = new Lua_script();    
    connect(script,SIGNAL(sig_stringMsg(QString)),this,SLOT(slot_scriptmessage(QString)));
    //connect(this,SIGNAL(executepro()),script,SLOT(execute()));
    //connect(this,SIGNAL(load_string(QString)),script,SLOT(load_string(QString)));
    connect(script,SIGNAL(sig_movej(QVector<double>,QString,QString,QString,QString,QString)),this,SLOT(movej(QVector<double>,QString,QString,QString,QString,QString)));
    //connect(this,SIGNAL(sig_Next()),script,SLOT(nextrun()));
    //connect(SysSignal::instance(),SIGNAL(sig_movej()),this,SLOT(movej()));

    edit = new CodeEditor();
    QHBoxLayout* layout=new QHBoxLayout(ui->frame_editer);
    layout->addWidget(edit);
    TagDataItem* item = new TagDataItem(MARKBLOCK, 10);
    item->markerAdd(2, Qt::blue);
    item->markerAdd(5, Qt::yellow);
    item->markerAdd(526, Qt::red);
    item->markerAdd(527, Qt::black);
    edit->AddTag(item);
    TagDataItem *itemNumber = new TagDataItem(NUMBER, 20);
    edit->AddTag(itemNumber);
    TagDataItem *item2 = new TagDataItem(MARKBLOCK, 10);
    item2->markerAdd(5, Qt::blue);
    item2->markerAdd(6, Qt::yellow);
    item2->markerAdd(7, Qt::red);
    item2->markerAdd(8, Qt::red);
    item2->markerAdd(526, Qt::red);
    item2->markerAdd(527, Qt::red);
    edit->AddTag(item2);
    QFont ft;
    ft.setFamily("微软雅黑");
    ft.setPointSize(10);
    edit->setFont(ft);

//    edit->UpdateColor(1,3,2, 5,Qt::red, Qt::yellow);
//    edit->UpdateColor(2,3,2, 5,Qt::red, Qt::yellow);

}

MainWindow::~MainWindow()
{
    delete ui;
    delete script;
}


void MainWindow::on_pushButton_connect_clicked()
{
    QString ip=ui->lineEdit_ip->text();
    Taike::PickMoveZ::Instance()->setip(ip);

    Taike::PickMoveZ::Instance()->connect();
    connect(Taike::PickMoveZ::Instance(),&Taike::PickMoveZ::networkconnected,[=](){
        refrshtimer->start(100);
    });

}


void MainWindow::on_pushButton_disconnect_clicked()
{
    Taike::PickMoveZ::Instance()->disconnect();
}

void MainWindow::updatejoint(QVector<double> joint)
{
    if(joint.size()<6) return;
    currentJoint=joint;
    ui->label_j1_vl->setText(QString::number(joint.at(0),'f',2));
    ui->label_j2_vl->setText(QString::number(joint.at(1),'f',2));
    ui->label_j3_vl->setText(QString::number(joint.at(2),'f',2));
    ui->label_j4_vl->setText(QString::number(joint.at(3),'f',2));
    ui->label_j5_vl->setText(QString::number(joint.at(4),'f',2));
    ui->label_j6_vl->setText(QString::number(joint.at(5),'f',2));
    //    QVector<double> pos=getcurrentPos();
    //    if(pos != currentPos)
    //        updatePos(pos);
}

void MainWindow::updatePos(QVector<double> pos)
{
    if(pos.size()<7) return;
    ui->label_x_vl->setText(QString::number(pos.at(0),'f',2));
    ui->label_y_vl->setText(QString::number(pos.at(1),'f',2));
    ui->label_z_vl->setText(QString::number(pos.at(2),'f',2));
    Quaternion quater;
    quater.u0=pos.at(3);
    quater.u1=pos.at(4);
    quater.u2=pos.at(5);
    quater.u3=pos.at(6);

    QVector3D el= Taike::ToEulerAngles(quater);
    float r=el.x()*180/M_PI,p=el.y()*180/M_PI,y=el.z()*180/M_PI;



    currentPos.clear();
    currentPos.push_back(pos.at(0));
    currentPos.push_back(pos.at(1));
    currentPos.push_back(pos.at(2));
    currentPos.push_back(r);
    currentPos.push_back(p);
    currentPos.push_back(y);
    ui->label_a_vl->setText(QString::number(r,'f',2));
    ui->label_b_vl->setText(QString::number(p,'f',2));
    ui->label_c_vl->setText(QString::number(y,'f',2));
    //    QVector<double> joint=getcurrentJoint();
    //    if(currentJoint != joint)
    //        updatejoint(joint);
}

void MainWindow::updateacc(float acc)
{
    float v=ui->doubleSpinBox->value();
    if(acc==v)
        return;
    ui->doubleSpinBox->setValue(acc);
}

void MainWindow::updatedacc(float dacc)
{
    float v=ui->doubleSpinBox_2->value();
    if(dacc==v)
        return;
    ui->doubleSpinBox_2->setValue(dacc);
}

void MainWindow::updateProspeed(float speed)
{
    float v=ui->doubleSpinBox_speed->value();
    if(speed==v)
    {
        return;
    }
    ui->doubleSpinBox_speed->setValue(speed);
}

void MainWindow::updateJogmode(int md)
{
    //qDebug()<<"updatejogmode"<<md;
    int t=ui->comboBox_jogmode->currentIndex();
    if(t == md-1)
        return;
    currentmodel = md-1;
    ui->comboBox_jogmode->setCurrentIndex(md-1);
}

void MainWindow::slot_refresh()
{
    QVector<double> joint=Taike::getcurrentJoint();
    updatejoint(joint);
    QVector<double> pos=Taike::getcurrentPos();
    updatePos(pos);
    updateacc(jogacc);
    updatedacc(jogdacc);
    updateProspeed(prospeed);
    updateJogmode(jogmode);
}

void MainWindow::movej(QVector<double> targ, QString v, QString z, QString t, QString w, QString l)
{
    //QVector<double> target = SysSignal::instance()->gettarget("MoveAbsJ");
    qDebug()<<"MainWindow::movej() "<<targ<<v<<z<<t<<w<<l;
    //string z=  getzone();
    //string tool= mtool;
    //string vcc=  getvcc();
    //string wobj= mwobj;
    //cout<<"MainWindow::movej() "<< z<<vcc<<endl;

}


void MainWindow::on_pushButton_enable_clicked()
{
    Taike::PickMoveZ::Instance()->setenable(true);
}


void MainWindow::on_pushButton_disable_clicked()
{
    Taike::PickMoveZ::Instance()->setenable(false);
}


void MainWindow::on_pushButton_Emagen_clicked()
{
    Taike::PickMoveZ::Instance()->setEMode();
}


void MainWindow::on_pushButton_EReset_clicked()
{
    Taike::PickMoveZ::Instance()->setEReset();
}


void MainWindow::on_comboBox_currentIndexChanged(int index)
{
    Taike::PickMoveZ::Instance()->SetCoordSysterm(index);
}

void MainWindow::on_doubleSpinBox_speed_valueChanged(double arg1)
{
    Taike::PickMoveZ::Instance()->setprospeed(arg1);
}

void MainWindow::on_doubleSpinBox_2_valueChanged(double arg1)
{
    Taike::PickMoveZ::Instance()->setJogDAcc(arg1);
}


void MainWindow::on_doubleSpinBox_valueChanged(double arg1)
{
    Taike::PickMoveZ::Instance()->setJogAcc(arg1);
}


void MainWindow::on_comboBox_jogmode_currentIndexChanged(int index)
{
    qDebug()<<"jog mode changed "<<index;
    if(currentmodel == index)
        return;
    Taike::PickMoveZ::Instance()->setjogmodel(index);

}


void MainWindow::on_pushButton_j1_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(0,-1);
}
void MainWindow::on_pushButton_j1_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(0,1);
}

void MainWindow::on_pushButton_j2_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(1,-1);
}


void MainWindow::on_pushButton_j3_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(2,-1);
}


void MainWindow::on_pushButton_j4_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(3,-1);
}


void MainWindow::on_pushButton_j5_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(4,-1);
}


void MainWindow::on_pushButton_j6_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(5,-1);
}


void MainWindow::on_pushButton_j2_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(1,1);
}


void MainWindow::on_pushButton_j3_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(2,1);
}


void MainWindow::on_pushButton_j4_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(3,1);
}


void MainWindow::on_pushButton_j5_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(4,1);
}


void MainWindow::on_pushButton_j6_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogJoint(5,1);
}


void MainWindow::on_pushButton_x_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(0,-1);
}


void MainWindow::on_pushButton_y_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(1,-1);
}


void MainWindow::on_pushButton_z_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(2,-1);
}


void MainWindow::on_pushButton_a_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(3,-1);
}


void MainWindow::on_pushButton_b_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(4,-1);
}


void MainWindow::on_pushButton_c_left_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(5,-1);
}


void MainWindow::on_pushButton_x_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(0,1);
}


void MainWindow::on_pushButton_y_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(1,1);
}


void MainWindow::on_pushButton_z_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(2,1);
}


void MainWindow::on_pushButton_a_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(3,1);
}


void MainWindow::on_pushButton_b_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(4,1);
}


void MainWindow::on_pushButton_c_right_pressed()
{
    Taike::PickMoveZ::Instance()->jogPos(5,1);
}


void MainWindow::on_pushButton_pp_get_clicked()
{
    QString str;
    str.append(QString::number(currentJoint.at(0)));
    for(int i=1;i<currentJoint.size();++i)
    {
        str.append(",").append(QString::number(currentJoint.at(i)));
    }
    ui->lineEdit_target_pp->setText(str);
}


void MainWindow::on_pushButton_line_get_clicked()
{
    QString str;
    str.append(QString::number(currentPos.at(0)));
    for(int i=1;i<currentPos.size();++i)
    {
        str.append(",").append(QString::number(currentPos.at(i)));
    }
    ui->lineEdit_target->setText(str);
}


void MainWindow::on_pushButton_arc_get_pressed()
{
    QString str;
    str.append(QString::number(currentPos.at(0)));
    for(int i=1;i<currentPos.size();++i)
    {
        str.append(",").append(QString::number(currentPos.at(i)));
    }
    ui->arc_target_1->setText(str);
}


void MainWindow::on_pushButton_arc_get_2_pressed()
{
    QString str;
    str.append(QString::number(currentPos.at(0)));
    for(int i=1;i<currentPos.size();++i)
    {
        str.append(",").append(QString::number(currentPos.at(i)));
    }
    ui->arc_target_2->setText(str);
}


void MainWindow::on_pushButton_pp_run_clicked()
{
    QString tar=ui->lineEdit_target_pp->text();
    QVector<double> target= string2vector(tar);
    Taike::PickMoveZ::Instance()->movepp(target);
}


void MainWindow::on_pushButton_line_run_clicked()
{
    QString tar=ui->lineEdit_target->text();
    QVector<double> target= string2vector(tar);
    Taike::PickMoveZ::Instance()->moveL(target);
}


void MainWindow::on_pushButton_arc_run_clicked()
{
    QString tar=ui->arc_target_1->text();
    QString tar2=ui->arc_target_2->text();
    QVector<double> pass = string2vector(tar);
    QVector<double> target= string2vector(tar2);
    Taike::PickMoveZ::Instance()->moveC(pass,target);
}


void MainWindow::on_pushButton_test_clicked()
{
    QVector<double> pos=Taike::getcurrentPos();
    qDebug()<<"org pos "<<pos;
    Quaternion quater;
    quater.u0=pos.at(3);
    quater.u1=pos.at(4);
    quater.u2=pos.at(5);
    quater.u3=pos.at(6);
    QVector3D el= Taike::ToEulerAngles(quater);
    qDebug()<<"euler angle =="<<el*180/M_PI;

    quater = Taike::ToQuaternion(el.z(),el.y(),el.x());
    qDebug()<<"calcu quater == "<<quater.u0<<quater.u1<<quater.u2<<quater.u3;
}


void MainWindow::on_pushButton_stop_clicked()
{
    Taike::PickMoveZ::Instance()->movestop();
}


void MainWindow::on_pushButton_clear_clicked()
{
    Taike::PickMoveZ::Instance()->clearerror();
}


void MainWindow::on_pushButton_pro_clicked()
{
    ui->stackedWidget->setCurrentIndex(1);
}


void MainWindow::on_pushButton_return_clicked()
{
    ui->stackedWidget->setCurrentIndex(0);
}


void MainWindow::on_radioButton_loop_clicked()
{
    if(ui->radioButton_loop->isChecked())
        Taike::PickMoveZ::Instance()->setProRunMode(false);
}


void MainWindow::on_radioButton_step_clicked()
{
    if(ui->radioButton_loop->isChecked())
        Taike::PickMoveZ::Instance()->setProRunMode(true);
}


void MainWindow::on_pushButton_reload_clicked()
{
    QString path= QFileDialog::getOpenFileName(this,"Open a program","./",tr("Pro File (*.lua *.txt)"));
    if(path.isEmpty())
        return;
    ui->label_title->setText(path);
    QFile file(path);
    if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        return;
    }
    QString prostr = file.readAll();
    file.close();

    if(!prostr.isEmpty())
        edit->setPlainText(prostr);
    else
        return;
    qDebug()<<"maingui thread id == "<<QThread::currentThreadId();
    if(_sim)
    {
        script->load_program(prostr);

    }
    else
    {
        Taike::PickMoveZ::Instance()->LoadProgram(path);
    }
}


void MainWindow::on_pushButton_prostatu_clicked()
{
    if(_sim)
    {
        int t;// = script->getProStatus();
        QString str;
        switch (t) {
        case 0:
            str= "FREE";
            break;
        case 1:
            str= "RUN";
            break;
        case 2:
            str= "STOP";
            break;
        case 3:
            str= "WAIT";
            break;
        case 4:
            str= "ERROR";
            break;
        default:
            break;
        }
        ui->label_statu->setText(str);
    }
    //Taike::PickMoveZ::Instance()->getProstatus();
}


void MainWindow::on_checkBox_sim_clicked()
{
    _sim= ui->checkBox_sim->isChecked();
}

void MainWindow::slot_scriptmessage(QString str)
{
    ui->label_info->setText(str);
}


void MainWindow::on_pushButton_start_clicked()
{
    script->runprogram();
}


void MainWindow::on_pushButton_step_clicked()
{
    script->nextrun();
}


void MainWindow::on_radioButton_io1_clicked()
{
    QString ioname="DO20_10";
    Taike::PickMoveZ::Instance()->SetDO(ioname,1);
}
